#include <cassert>
#include <vector>
#include <deque>
#include <queue>
#include <iostream>
#include <fstream>

#define CACHE_PATHS				// Store paths corresponding to abstract edges in the abstract search graph
// #define CHECK_PATHS			// Check the validity of paths returned by the algorithm
// #define CHECK_SUBOPT			// Report paths with high suboptimality

// Type definitions
typedef unsigned short	Cost;

typedef int				coord_type;
typedef unsigned char	region_index;
typedef unsigned char	border_index;
typedef unsigned char	vertex_index;

// Sentinel value to represent obstructions
const region_index OBSTRUCTION = 255;

// Structure to represent two dimensional locations and vectors
struct coord2
{
    coord_type x,y;

    coord2() {}
    coord2(coord_type x, coord_type y) : x(x), y(y) {}

    bool operator == (const coord2 & v) const { return x==v.x&&y==v.y; }
    bool operator != (const coord2 & v) const { return x!=v.x||y!=v.y; }

    coord2 operator + (const coord2 & v) const { return coord2(x+v.x,y+v.y); }
    coord2 operator - (const coord2 & v) const { return coord2(x-v.x,y-v.y); }
    coord2 operator * (coord_type f) const { return coord2(x*f,y*f); }
    coord2 operator / (coord_type f) const { return coord2(x/f,y/f); }

    static const coord2 directions[8];
    static const int costs[8];
    static const bool diag[8]; 
};

// Helper constants to assist in octile movement
const coord2 coord2::directions[8] = { coord2(1,0), coord2(1,1), coord2(0,1), coord2(-1,1), coord2(-1,0), coord2(-1,-1), coord2(0,-1), coord2(1,-1) };
const int coord2::costs[8] = {2,3,2,3,2,3,2,3};
const bool coord2::diag[8] = {false,true,false,true,false,true,false,true};

// Helper function to output vectors
std::ostream & operator<<(std::ostream & out, const coord2 & v)
{
    return out << "(" << v.x << "," << v.y << ")";
}

// Return twice the octile distance between two points
// This allows the distance to be returned as an integer instead of a real
int GetOctileDistanceTimesTwo(const coord2 & a, const coord2 & b)
{
    const int diagonal(3);
    const int straight(2);

    const int dx(abs(b.x-a.x));
    const int dy(abs(b.y-a.y));
  
    if(dx > dy)
    {
        return dy * diagonal + (dx - dy) * straight;
    }
    else
    {
        return dx * diagonal + (dy - dx) * straight;
    }  
}

// Get the cost of a path if each pair of vertices is traveled between in an octile fashion
int GetOctilePathCostTimesTwo(const std::vector<coord2> & path)
{
    int sumCost(0);
    for(size_t i(1); i<path.size(); ++i)
    {
        sumCost += GetOctileDistanceTimesTwo(path[i-1], path[i]);
    }
    return sumCost;
}

// Use Bresenham's line drawing algorithm to write out a line from one location to another
void WriteLine(const coord2 & v0, const coord2 & v1, std::vector<coord2> & points) 
{
    coord_type x0(v0.x),x1(v1.x),y0(v0.y),y1(v1.y);
    coord_type Dx = x1 - x0; 
    coord_type Dy = y1 - y0;
    const bool steep(abs(Dy) >= abs(Dx));
    if (steep) 
    {
        std::swap(x0, y0);
	std::swap(x1, y1);
	// recompute Dx, Dy after swap
	Dx = x1 - x0;
	Dy = y1 - y0;
    }
    coord_type xstep(1), ystep(1);
    if (Dx < 0) { xstep = -1; Dx = -Dx; }
    if (Dy < 0) { ystep = -1; Dy = -Dy; }
    coord_type TwoDy = 2*Dy; 
    coord_type TwoDyTwoDx = TwoDy - 2*Dx; // 2*Dy - 2*Dx
    coord_type E = TwoDy - Dx; //2*Dy - Dx
    coord_type y = y0;
    for (coord_type x = x0; x != x1; x += xstep) 
    {
        points.push_back(steep ? coord2(y,x) : coord2(x,y));
	// next
	if (E > 0) 
	{
	    E += TwoDyTwoDx; //E += 2*Dy - 2*Dx;
	    y = y + ystep;
	} 
	else 
	{
	    E += TwoDy; //E += 2*Dy;
	}
    }
}

// Structure containing a loaded map and all necessary auxiliary structures
class Map
{
	// Structure represents a region in the map
    struct Region
    {
        std::vector<border_index> borders;		// The borders that surround this region
    };

	// Structure represents a border between two regions
    struct Border
    {
        std::vector<int> vertices;				// The vertices that lay on this border
    };

	// Structure representing a directed edge in the abstract search graph
    struct GraphEdge
    {
        int nextVertex; // The vertex that this edge points to
        bool straight;  // True if we can follow this edge via a straight line
        int cost;		// The true cost of moving along this edge

#ifdef CACHE_PATHS
        std::vector<coord2> path;	// A precomputed path along this edge
#endif

        GraphEdge() {}
        GraphEdge(int nextVertex, int cost, bool straight) : nextVertex(nextVertex), straight(straight), cost(cost) {}

		// Assignment operator, not strictly needed
		const GraphEdge & operator = (const GraphEdge & rhs) 
		{
			nextVertex   = rhs.nextVertex;
			straight     = rhs.straight;
			cost         = rhs.cost;
#ifdef CACHE_PATHS
			path         = rhs.path;
#endif
			return *this;
		}
    };

	// Structure representing a vertex in the abstract search graph
    struct GraphVertex
    {
        coord2 position;						// The location of this vertex
        std::vector<GraphEdge> adjacencies;		// Directed edges traveling to this vertex's neighbours

        GraphVertex(const coord2 & position) : position(position) {}
    };

    coord_type width,height;
    std::vector<region_index> map;

    std::vector<Region> regions;
    std::vector<Border> borders;

    int numVertices;
    std::vector<GraphVertex> vertices;
    std::vector<GraphEdge*> edgePtrs;

#ifdef CACHE_PATHS
    size_t sumStoredPathLength;
#endif
    size_t numEdges;

	// Helper functions to access map data
	// Primarily helped during development to simplify experiments
	// in reorganising map data structures.
    int GetMapIndex(coord_type x, coord_type y) { return y*width+x; }
    int GetMapIndex(const coord2 & v) { return GetMapIndex(v.x,v.y); }
    int GetMapSize() { return width*height; }
    region_index & GetMapRegion(coord_type x,coord_type y) { return map[GetMapIndex(x,y)]; }
    GraphVertex & GetVertex(int v) { return vertices[v]; }
    GraphEdge * & GetEdgePtr(int v0, int v1) { return edgePtrs[v1*numVertices+v0]; }
    GraphEdge & GetEdge(int v0, int v1) 
    {
      GraphEdge * edge(GetEdgePtr(v0,v1));
      assert(edge != 0);
      return *edge;
    }

    const std::vector<border_index> & GetRegionBorders(int r) const { return regions[r].borders; }
    const coord2 & GetVertexPosition(int v) const { return vertices[v].position; }
    const std::vector<GraphEdge> & GetVertexAdjacencies(int v) const { return vertices[v].adjacencies; }

	// Trace a path using Bresenham's line drawing algorithm. Return false if the line could not be
	// completed due to an obstruction.
    bool IsEdgeBlocked(const coord2 & point0, const coord2 & point1, std::vector<coord2> & points);

	// Connect two vertices with directed edges.
    void AddEdge(int vertex0, int vertex1)
    {
        GraphVertex & v0(GetVertex(vertex0));
		GraphVertex & v1(GetVertex(vertex1));

		// Determine the straight line cost and whether such a line is feasible
        std::vector<coord2> path;
        const bool blocked(IsEdgeBlocked(v0.position,v1.position,path));
		const int cost(GetOctileDistanceTimesTwo(v0.position,v1.position));

		// Allocate two new edges using this information
		v0.adjacencies.push_back(GraphEdge(vertex1,cost,!blocked));
		v1.adjacencies.push_back(GraphEdge(vertex0,cost,!blocked));
		numEdges += 2;

		GraphEdge & edge0(v0.adjacencies.back());
		GraphEdge & edge1(v1.adjacencies.back());

		// If blocked, calculate and store true cost, and possibly cache true path
        if(blocked)
        {
            path.clear();
            SearchMap(v0.position,v1.position,path);
#ifdef CACHE_PATHS
            edge0.path = path;
            sumStoredPathLength += edge0.path.size();
#endif
            path.push_back(v1.position);
            edge0.cost = edge1.cost = GetOctilePathCostTimesTwo(path);
#ifdef CACHE_PATHS
            std::copy(path.rbegin(), path.rend(), std::back_inserter(edge1.path));
            edge1.path.pop_back();
            sumStoredPathLength += edge1.path.size();
#endif
        }
    }
public:
    Map(const char * filename)
    {
		std::ifstream in(filename);
		if(in)
		{
			// load map data
			in >> width >> height;
			map.resize(width*height);
			for(coord_type y(0); y<height; ++y) 
			{
				for(coord_type x(0); x<width; ++x) 
				{
					int region; in >> region;
					GetMapRegion(x,y) = (region_index)region;
				}
			}

			// load regions
			int numRegions; in >> numRegions;
			std::cout << "Reading " << numRegions << " regions" << std::endl;
			for(int i(0); i<numRegions; ++i) 
			{
				int numBorders; in >> numBorders;
				Region region;
				for(int j(0); j<numBorders; ++j) 
				{
					int border; in >> border;
					region.borders.push_back(border);
				}
				regions.push_back(region);
			}

			// load borders
			int numBorders; in >> numBorders;
			std::cout << "Reading " << numBorders << " borders" << std::endl;
			for(int i(0); i<numBorders; ++i) 
			{
				coord2 start,end;
				in >> start.x >> start.y >> end.x >> end.y;

				Border border; 
				const int cost(GetOctileDistanceTimesTwo(start,end));
				//std::cout << (cost*0.5f) << std::endl;
				if(cost > 90)
				{
					coord_type gap(8);
					coord_type ws(gap);
					coord_type we(cost-gap);

					border.vertices.push_back((int)vertices.size());
					vertices.push_back(GraphVertex((start*ws+end*we)/cost));

					border.vertices.push_back((int)vertices.size());
					vertices.push_back(GraphVertex( (start+end)/2 ));

					border.vertices.push_back((int)vertices.size());
					vertices.push_back(GraphVertex((start*we+end*ws)/cost));
				}
				else if(cost > 36)
				{
					border.vertices.push_back((int)vertices.size());
					vertices.push_back(GraphVertex((start*2+end)/3));

					border.vertices.push_back((int)vertices.size());
					vertices.push_back(GraphVertex((start+end*2)/3));
				}
				else
				{
					border.vertices.push_back((int)vertices.size());
					vertices.push_back(GraphVertex((start+end)/2));
				}
				borders.push_back(border);
			}
			numVertices = (int)vertices.size();
			std::cout << "Generated " << numVertices << " vertices" << std::endl;

			// generate edges
#ifdef CACHE_PATHS
			sumStoredPathLength = 0;
#endif
			numEdges = 0;
			for(int i(0); i<regions.size(); ++i) 
			{
				const std::vector<border_index> & regBorders(regions[i].borders);
				for(int j(0); j<regBorders.size(); ++j) 
				{
					for(int k(j+1); k<regBorders.size(); ++k) 
					{
						const std::vector<int> & vertices0(borders[regBorders[j]].vertices);
						const std::vector<int> & vertices1(borders[regBorders[k]].vertices);
						for(int i0(0); i0<vertices0.size(); ++i0)
						{
							for(int i1(0); i1<vertices1.size(); ++i1)
							{
								AddEdge(vertices0[i0], vertices1[i1]);
							}
						}
					}
				}
			}
			std::cout << "Generated " << numEdges << " edges" << std::endl;
#ifdef CACHE_PATHS
			std::cout << "Generated " << sumStoredPathLength << " vertices for stored paths" << std::endl;
#endif

			// cache pointers to edges
			edgePtrs.resize(numVertices*numVertices,0);	  
			for(int i(0); i<numVertices; ++i)
			{
				for(int j(0); j<vertices[i].adjacencies.size(); ++j)
				{
					GetEdgePtr(i,vertices[i].adjacencies[j].nextVertex) = &vertices[i].adjacencies[j];
				}
			}
		}
    }

	// Represent a node on the open list when searching the abstraction
    struct Node
    {
        int vertex;
        int parent;
        int gcost;
        int fcost;

        Node() {}
        Node(int vertex, int parent, int gcost, int fcost) : vertex(vertex), parent(parent), gcost(gcost), fcost(fcost) {}

        bool operator < (const Node & rhs) const
        { 
			if(fcost > rhs.fcost) return true;
			return (fcost == rhs.fcost && gcost > rhs.gcost);
		}
    };

	// Search the abstract graph and produce a complete path if possible
    bool SearchAbstraction(const coord2 & start, const coord2 & goal, std::vector<coord2> & path)
    {
        // Determine regions of path endpoints
        const int startRegion(GetMapRegion(start.x,start.y));
        const int goalRegion(GetMapRegion(goal.x,goal.y));

        // Reject search if points in obstruction
        if(startRegion == OBSTRUCTION || goalRegion == OBSTRUCTION)
        {
            return false;
        }

        // Produce trivial path if points in same region
        if(startRegion == goalRegion)
        {
            if(IsEdgeBlocked(start, goal, path))
            {
                path.clear();
                SearchMap(start, goal, path);
            }
            path.push_back(goal);
            return true;
        }

        // Flag goal border vertices
        std::vector<bool> isGoal(numVertices, false);
        {
            const std::vector<border_index> & goalBorders(GetRegionBorders(goalRegion));
            for(size_t i(0); i<goalBorders.size(); ++i)
            {
                const std::vector<int> & vertices(borders[goalBorders[i]].vertices);
                for(size_t j(0); j<vertices.size(); ++j)
                {
                    isGoal[vertices[j]] = true;
                }
            }
        }

        // Initialise open list with borders of start region
        std::priority_queue<Node> open;
        {
            const std::vector<border_index> & startBorders(GetRegionBorders(startRegion));
            for(size_t i(0); i<startBorders.size(); ++i)
            {        
                const std::vector<int> & vertices(borders[startBorders[i]].vertices);
                for(size_t j(0); j<vertices.size(); ++j)
                {
                    const int nextVertex(vertices[j]);
                    const coord2 & nextPosition(GetVertexPosition(nextVertex));
           
                    const int gcost(GetOctileDistanceTimesTwo(start, nextPosition));
                    const int hcost(GetOctileDistanceTimesTwo(nextPosition, goal));
                    const int fcost(gcost + hcost);
            
                    open.push(Node(nextVertex, -1, gcost, fcost));
                }
            }
        }
        
        // Do a best first search
        std::vector<bool> closed(numVertices, false);
        std::vector<int> parent(numVertices, -1);
        while(!open.empty())
        {
            // Obtain the node with least fcost
            const Node current(open.top()); open.pop();

            // If we have not already closed this node
            if(!closed[current.vertex])
            {               
                // If we have reached the goal
                if(current.vertex == -1)
                {
                    // Reconstruct the path by walking backwards
                    std::deque<int> pathVertices;
                    int vertex(current.parent);
                    while(vertex != -1)
                    {
                        pathVertices.push_front(vertex);
                        vertex = parent[vertex];
                    }
                    
                    // Output the path
					assert(!pathVertices.empty());

					// If the abstract path is short, do a direct search
					if(GetOctileDistanceTimesTwo(start,goal) <= 128 &&
						pathVertices.size() <= 3)
					{
						if(IsEdgeBlocked(start, goal, path))
						{
							path.clear();
							SearchMap(start, goal, path);
						}
						path.push_back(goal);
						return true;
					}
					else // Path is long, use abstract path
					{
						const coord2 & front(GetVertex(pathVertices.front()).position);
						const coord2 & back(GetVertex(pathVertices.back()).position);

						// Build first leg of path, from start to first border
						size_t pathSize(path.size());
						if(IsEdgeBlocked(start, front, path))
						{
							path.resize(pathSize);
							SearchMap(start, front, path);
						}
						// Build successive legs of paths using straight lines and cached sequences
						for(size_t i(1); i<pathVertices.size(); ++i)
						{
							const int vertex0(pathVertices[i-1]);
							const int vertex1(pathVertices[i]);
							const coord2 & pos0(GetVertex(vertex0).position);
							const coord2 & pos1(GetVertex(vertex1).position);
							const GraphEdge & edge(GetEdge(vertex0,vertex1));
							if(edge.straight)
							{                        
								WriteLine(pos0,pos1,path);
							}
							else
							{
#ifdef CACHE_PATHS
								std::copy(edge.path.begin(), edge.path.end(), std::back_inserter(path));
#else
								SearchMap(pos0,pos1,path);
#endif
							}
						}
						// Build the final leg of the path, from border to goal
						pathSize = path.size();
						if(IsEdgeBlocked(back, goal, path))
						{
							path.resize(pathSize);
							SearchMap(back, goal, path);
						}
						path.push_back(goal);
					}

					// Return success
					return true;
                }

                // Otherwise, close the current node
                closed[current.vertex] = true;
                parent[current.vertex] = current.parent;

                // If we are bordering the goal region
                if(isGoal[current.vertex])
                {
                    // Enqueue a straight line segment to the goal
                    const coord2 & currentPos(GetVertexPosition(current.vertex));
                    const int gcost(current.gcost + GetOctileDistanceTimesTwo(currentPos, goal));                            
                    open.push(Node(-1, current.vertex, gcost, gcost));
                }
                else // If we do not border the goal region
                {
                    // Enqueue straight lines to all our adjacencies
                    const std::vector<GraphEdge> & adjacencies(GetVertex(current.vertex).adjacencies);
                    for(size_t i(0); i<adjacencies.size(); ++i)
                    {
                        // If an adjacent vertex is not already closed
                        const int nextVertex(adjacencies[i].nextVertex);
                        if(!closed[nextVertex])
                        {                           
                            // Calculate the appropriate consts
                            const coord2 & nextPos(GetVertex(nextVertex).position);
                            const int gcost(current.gcost + adjacencies[i].cost);
                            const int hcost(GetOctileDistanceTimesTwo(nextPos, goal));
                            const int fcost(gcost + hcost);
                            
                            // And enqueue the node
                            open.push(Node(nextVertex, current.vertex, gcost, fcost));
                        }
                    }
                }
            }
        }

        // Search exhausted, report failure
        return false;
    }

	// Represent a node on the open list in our direct A* search
    struct Node2
    {
        coord2 vertex;
        coord2 parent;
        int gcost;
        int fcost;
        
        Node2() {}
        Node2(const coord2 & vertex, const coord2 & parent, int gcost, int fcost) : vertex(vertex), parent(parent), gcost(gcost), fcost(fcost) {}
        
        bool operator < (const Node2 & rhs) const
        {
            if(fcost > rhs.fcost) return true;
            return (fcost == rhs.fcost && gcost > rhs.gcost);
        }
    };

	// Use a direct A* search to find a path. NOTE: This method does NOT push the goal state
	// onto the path which is returned. This is to facilitate its use in a number of situations.
    bool SearchMap(const coord2 & start, const coord2 & goal, std::vector<coord2> & path)
    {
        int numOpen(0), numClosed(0);

        // Initialise open list with start
        std::priority_queue<Node2> open;
        {
	  	    const int hcost(GetOctileDistanceTimesTwo(start, goal));            
            open.push(Node2(start, coord2(), 0, hcost));
		    ++numOpen;
        }

        // Do a best first search
        std::vector<bool> closed(GetMapSize(), false);
        std::vector<coord2> parent(GetMapSize());
        while(!open.empty())
        {
            // Obtain the node with least fcost
            const Node2 current(open.top()); open.pop();

            // If we have not already closed this node
		    const int index(GetMapIndex(current.vertex));
			assert(index >= 0 && index < GetMapSize());
            if(!closed[index])
            {               
                // Otherwise, close the current node
                closed[index] = true;
                parent[index] = current.parent;
				++numClosed;

                // If we have reached the goal
                if(current.vertex == goal)
                {
                    // Reconstruct the path by walking backwards
                    std::deque<coord2> tempPath;
                    coord2 vertex(current.vertex);
					while(vertex != start)
					{
						vertex = parent[GetMapIndex(vertex)];
						tempPath.push_front(vertex);
					}
                    
                    // Output the path
                    std::copy(tempPath.begin(), tempPath.end(), std::back_inserter(path));

                    // Return success
                    return true;
                }

                // Enqueue all neighbours
                const coord2 & curPos(current.vertex);
                for(size_t i(0); i<8; ++i)
				{
					// Determine if we can move in this direction
					const coord2 & dir(coord2::directions[i]);
					const coord2 next(curPos + dir);
					const int nextIndex(GetMapIndex(next));
					if(map[nextIndex] == OBSTRUCTION) continue;
					if(coord2::diag[i])
					{
						if(GetMapRegion(curPos.x+dir.x, curPos.y) == OBSTRUCTION) continue;
						if(GetMapRegion(curPos.x, curPos.y+dir.y) == OBSTRUCTION) continue;
					}
					if(closed[nextIndex]) continue;

					// Calculate the appropriate consts
					const int gcost(current.gcost + coord2::costs[i]);
					const int hcost(GetOctileDistanceTimesTwo(next, goal));
					const int fcost(gcost + hcost);
					    
					// And enqueue the node
					open.push(Node2(next, current.vertex, gcost, fcost));
					++numOpen;
				}
			}
		}

        // Search exhausted, report failure
        return false;
    }

	// Check a path for validity
    bool CheckPath(const coord2 & start, const coord2 & goal, 
                   const std::vector<coord2> & path)
    {
        if(path.empty())
        {
            std::cout << "CheckPath() : path is empty" << std::endl;
            return false;
        }
        if(path.front() != start)
        {
            std::cout << "CheckPath() : wrong start" << std::endl;
            return false;
        }
        if(path.back() != goal)
        {
            std::cout << "CheckPath() : wrong goal" << std::endl;
            return false;
        }
        for(size_t i(0); i<path.size(); ++i)
        {
            const coord2 & p(path[i]);
            if(GetMapRegion(p.x,p.y) == OBSTRUCTION)
            {
                std::cout << "CheckPath() : hits obstruction" << std::endl;
                return false;
            }
        }
        for(size_t i(1); i<path.size(); ++i)
        {
            const coord2 & p0(path[i-1]);
            const coord2 & p1(path[i]);
            if(GetMapRegion(p0.x,p1.y) == OBSTRUCTION ||
               GetMapRegion(p1.x,p0.y) == OBSTRUCTION)
            {
                std::cout << "CheckPath() : cuts corners" << std::endl;
                return false;
            }
            if(GetOctileDistanceTimesTwo(p0,p1) > 3)
            {
                std::cout << "CheckPath() : contains gaps" << std::endl;
                return false;
            }
        }
        return true;
    }
};

// Trace a straight line using Bresenham's line drawing algorithm, outputting coordinates as we go.
// If we encounter an impassable obstacle, return false. This algorithm allows us to quickly add
// certain lines to a full path, and gives us a fallback if those lines cannot be added.
bool Map::IsEdgeBlocked(const coord2 & point0, const coord2 & point1, std::vector<coord2> & points)
{
    coord_type x0(point0.x),x1(point1.x);
    coord_type y0(point0.y),y1(point1.y);
    coord_type Dx = x1 - x0;
    coord_type Dy = y1 - y0;
    const bool steep(abs(Dy) >= abs(Dx));
    if (steep)
    {
		std::swap(x0, y0);
		std::swap(x1, y1);
        // recompute Dx, Dy after swap
        Dx = x1 - x0;
        Dy = y1 - y0;
    }
    coord_type xstep(1), ystep(1);
    if (Dx < 0) { xstep = -1; Dx = -Dx; }
    if (Dy < 0) { ystep = -1; Dy = -Dy; }
    coord_type TwoDy = 2*Dy;
    coord_type TwoDyTwoDx = TwoDy - 2*Dx; // 2*Dy - 2*Dx
    coord_type E = TwoDy - Dx; //2*Dy - Dx
    coord_type y = y0;
    for (coord_type x = x0; x != x1; x += xstep)
    {
        const coord2 point(steep ? coord2(y,x) : coord2(x,y));
        if(GetMapRegion(point.x,point.y) == OBSTRUCTION) return true;
        points.push_back(point);

        // next
        if (E > 0)
		{
            E += TwoDyTwoDx; //E += 2*Dy - 2*Dx;
            if((steep ? GetMapRegion(y,x+xstep) : GetMapRegion(x+xstep,y)) == OBSTRUCTION) return true;
            if((steep ? GetMapRegion(y+ystep,x) : GetMapRegion(x,y+ystep)) == OBSTRUCTION) return true;
            y = y + ystep;
		}
        else
		{
            E += TwoDy; //E += 2*Dy;
		}
    }
    return false;
}

#include <ctime>
#include <sys/time.h>
#include <sys/resource.h>
#include <stdio.h>

// Test our algorithm on 10000 problems and report several statistics
void TestSuite(Map & map)
{
    double min(1000000);
    double max(1);
    double num(0);
    int count(0);
    int failed(0);

    double sumOurCost(0);
    double sumTrueCost(0);

#ifdef CHECK_PATHS
    int good(0);
    int bad(0);
#endif

    rusage r_usage_start;
    rusage r_usage;

    getrusage(RUSAGE_SELF, &r_usage_start);

    clock_t startTime(clock());
    std::ifstream in("test.txt");
    while(in)
    {
        coord2 start,goal;
        in >> start.x >> start.y >> goal.x >> goal.y;
        double trueCost;
        in >> trueCost;
        if(in.bad()) break;

        std::vector<coord2> path;
        if(map.SearchAbstraction(start, goal, path))
        {
#ifdef CHECK_PATHS
            if(map.CheckPath(start, goal, path))
            {
                ++good;
            }
            else
            {
                std::cout << "Invalid path produced from ";
                std::cout << start << " to " << goal << std::endl;
                ++bad;
            }
#endif

            double ourCost(GetOctilePathCostTimesTwo(path)*0.5f);
            double subOpt(ourCost/trueCost);

			sumOurCost += ourCost;
			sumTrueCost += trueCost;

#ifdef CHECK_SUBOPT
			if (subOpt > 1.2f) 
			{
				std::cout << "Suboptimal path from " << start << " to " << goal << std::endl;
			}
#endif

            min = std::min(min, subOpt);
            max = std::max(max, subOpt);
            num += subOpt;
        }
        else
        {
            ++failed;
        }

        ++count;
		if(count % 10000 == 0) break;
    }
    clock_t endTime(clock());
    getrusage(RUSAGE_SELF, &r_usage);

    double elapsedCPUTime = (double)(r_usage.ru_utime.tv_sec - r_usage_start.ru_utime.tv_sec) + 
      (r_usage.ru_utime.tv_usec - r_usage_start.ru_utime.tv_usec)*0.000001;
    
    double elapsedTime((double)(endTime-startTime)/CLOCKS_PER_SEC);

    std::cout << "Number of paths searched = " << count << std::endl;
    std::cout << "Number of searches failed = " << failed << std::endl;
#ifdef CHECK_PATHS
    std::cout << "Number of good paths found = " << good << std::endl;
    std::cout << "Number of bad paths found = " << bad << std::endl;
#endif
    std::cout << "Minimum suboptimality = " << min << std::endl;
    std::cout << "Maximum suboptimality = " << max << std::endl;
    std::cout << "Average suboptimality = " << (num/count) << std::endl;
    std::cout << "Total excess cost = " << sumOurCost - sumTrueCost << std::endl;
    std::cout << "Suboptimality over " << count << " paths = " << (sumOurCost /sumTrueCost) << std::endl;
    std::cout << "Average time to search = " << (elapsedTime*1000/count) << " ms" << std::endl;
    std::cout << "Total time to search = " << elapsedTime << " s" << std::endl;
    std::cout << "Average CPU time to search = " << (elapsedCPUTime*1000/count) << " ms" << std::endl;
    std::cout << "Total CPU time to search = " << elapsedCPUTime << " s" << std::endl;
}

// Entry point, allow users to search specific paths or run a testing suite
// that summarises the effectiveness of this algorithm
int main()
{
    Map map("graph2.dat");

    std::cout << "1) Search with abstraction" << std::endl;
    std::cout << "2) Search without abstraction" << std::endl;
    std::cout << "3) Testing suite" << std::endl;
    
    int choice; std::cin >> choice;
    if(choice == 3) 
    { 
		TestSuite(map); 
    }
    else
    {
        coord2 start,goal;
        std::cin >> start.x >> start.y >> goal.x >> goal.y;
        std::vector<coord2> path;
        bool result(false);
        
        if(choice == 1) result = map.SearchAbstraction(start, goal, path);
        else if(choice == 2) result = map.SearchMap(start, goal, path);
        
        if(result)
        {
            path.push_back(goal);
            double ourCost(GetOctilePathCostTimesTwo(path)*0.5f);
            std::cout << "Path found with cost " << ourCost << std::endl;
        }
        else
        {
            std::cout << "Path not found" << std::endl;
        }
    }

    return 0;
}
